import heapq
import math

# 定义地图，示例一个简单的5x5方格地图，1表示障碍，0表示可通行
map_grid = [
    [0, 0, 0, 0, 0],
    [0, 1, 0, 1, 0],
    [0, 0, 0, 1, 0],
    [0, 1, 0, 0, 0],
    [0, 0, 0, 0, 0]
]

# 定义起点和终点坐标
start = (0, 0)
end = (4, 4)


# 计算欧几里得距离作为启发式函数（考虑可以走斜线，更贴合实际情况）
def heuristic(node, goal):
    return math.sqrt((node[0] - goal[0]) ** 2 + (node[1] - goal[1]) ** 2)


# A*算法主体
def astar(map_grid, start, end):
    # 定义八个方向的移动（上、下、左、右、左上、右上、左下、右下）
    directions = [(-1, 0), (1, 0), (0, -1), (0, 1), (-1, -1), (-1, 1), (1, -1), (1, 1)]
    # 移动成本，水平和垂直方向成本为1，对角线方向成本为1.4（近似sqrt(2)）
    costs = {
        (-1, 0): 1,
        (1, 0): 1,
        (0, -1): 1,
        (0, 1): 1,
        (-1, -1): 1.4,
        (-1, 1): 1.4,
        (1, -1): 1.4,
        (1, 1): 1.4
    }

    # 优先队列，存储待探索的节点，元素为 (f值, 当前节点坐标)
    open_list = []
    # 将起始节点加入优先队列，计算其f值（g值初始为0，f值=g值+h值）
    heapq.heappush(open_list, (heuristic(start, end), start))

    # 记录节点的g值（从起点到该节点的实际代价），初始为字典，起点g值设为0
    g_score = {start: 0}
    # 记录节点的父节点，用于路径回溯
    came_from = {}

    while open_list:
        # 从优先队列中取出f值最小的节点
        current_f, current = heapq.heappop(open_list)

        if current == end:
            # 如果当前节点就是终点，通过回溯构建路径并返回
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            path.append(start)
            return path[::-1]

        for i in range(len(directions)):
            direction = directions[i]
            # 探索相邻节点
            neighbor = (current[0] + direction[0], current[1] + direction[1])
            # 判断相邻节点是否在地图范围内
            if 0 <= neighbor[0] < len(map_grid) and 0 <= neighbor[1] < len(map_grid[0]):
                # 判断相邻节点是否为障碍物
                if map_grid[neighbor[0]][neighbor[1]] == 0:
                    # 计算到相邻节点的临时g值（考虑不同方向的移动成本）
                    tentative_g_score = g_score[current] + costs[direction]
                    if neighbor not in g_score or tentative_g_score < g_score[neighbor]:
                        # 如果是新节点或者找到了更优到达该节点的路径
                        g_score[neighbor] = tentative_g_score
                        # 计算相邻节点的f值（f值 = g值 + h值）
                        f_score = tentative_g_score + heuristic(neighbor, end)
                        # 将相邻节点加入优先队列
                        heapq.heappush(open_list, (f_score, neighbor))
                        # 记录相邻节点的父节点
                        came_from[neighbor] = current
    return None  # 如果没有找到路径则返回None


# 运行A*算法并输出结果
result_path = astar(map_grid, start, end)
if result_path:
    print("找到路径:", result_path)
else:
    print("未找到路径")
